// **********************************************************
// *************   Lay-Z Nunchuck RC Control Code   *********
// ***   Lay-Z Designed by Melih Karakelle on 2010-2012   ***
// **  an Arudino based Wii Nunchuck & MotionPlus Reader  ***
// **       This Source code licensed under GPL            **
// **********************************************************
// Version Number     : 1.00
// Latest Code Update : 2012-01-12
// Supported Hardware : Lay-Z boards (store.flytron.com)
// Project Forum      : http://forum.flytron.com
// **********************************************************
// # PROJECT DEVELOPERS # 
// Melih Karakelle (http://www.flytron.com) (forum nick name: Flytron)


// USAGE
// Use compatible trainer plug for your transmitter
// Connect (-) and (+) pins to supply pins of your transmitter
// Connect (S) pin to your transmitter's PPM-INPUT pin.
// Plug a Wii Nunchuck to Lay-Z
// Power on your transmitter and configure the trainer port as input and select the right (1-6) channels.
// Pull the trainer switch of your Tx for enabling trainer port.


#include <math.h>

#include "Wire.h"
#include "WiiChuck.h"


int ROLL;
int PITCH;
int JOYX;
int JOYY;
int BTNC;
int BTNZ;


#define MAXANGLE 90
#define MINANGLE -90
#define BLUE_LED 13

#define Channel_Count  6
#define PPM_Type  0
// 0 = NegativePPM  1=PositivePPM


WiiChuck chuck = WiiChuck();
int angleStart, currentAngle;
int tillerStart = 0;
double angle;


unsigned int Servo_Position[10] = {1500,1500,1500,1500,1500,1500,1500,1500,1500,1500};	//real servo position values
static unsigned char Servo_Number = 0;
unsigned int total_ppm_time=0;

void INIT_SERVO_DRIVER(void)
{
   TCCR1B   =   0x00;   //stop timer
   TCNT1H   =   0x00;   //setup
   TCNT1L   =   0x00;
   ICR1   =   40000;   // used for TOP, makes for 50 hz
   
   TCCR1A   =   0x02;   
   TCCR1B   =   0x1A; //start timer with 1/8 prescaler for 0.5us PPM resolution
   
   TIMSK1 = _BV (TOIE1);   
} 

ISR(TIMER1_OVF_vect)
  {
  unsigned int us; // this value is not real microseconds, we are using 0.5us resolution (2048 step), this is why the all values 2 times more than real microseconds.
  
  if (PPM_Type==0)
      PORTB &= ~_BV(1);
      else
      PORTB |= _BV(1);
  
  Servo_Number++; // jump to next servo
  if (Servo_Number>Channel_Count) // back to the first servo 
    {
    total_ppm_time = 0; // clear the total servo ppm time
    Servo_Number=0;
    } 

  if (Servo_Number == Channel_Count)  // Check the servo number. 
      {
        //Servos accepting 50hz ppm signal, this is why we are waiting for 20ms before second signal brust. 
        us = 40000 - total_ppm_time; //wait for total 20ms loop.  waiting time = 20.000us - total servo times

      }
      else
        us = 2 * Servo_Position[Servo_Number]; // read the servo timing from buffer
  
  total_ppm_time += us; // calculate total servo signal times.
  
  delayMicroseconds(300);
 
  if (PPM_Type==0)
      PORTB |= _BV(1);
      else
      PORTB &= ~_BV(1);
      
  TCNT1 = (40000 + 600) - us; // configure the timer interrupt for X micro seconds     
}


void setup() {
  Serial.begin(115200);
  chuck.begin();
  chuck.update();
  //chuck.calibrateJoy();
  
  pinMode(9,OUTPUT); //B1 pin is PPM Out
  
  INIT_SERVO_DRIVER();
}


void loop() {
  digitalWrite(BLUE_LED, LOW);
  delay(15);
  digitalWrite(BLUE_LED, HIGH);
  chuck.update(); 

  ROLL = map(chuck.readRoll(), -30, 30, 1000, 2000);
  PITCH = map(chuck.readPitch(), 60, 120, 1000, 2000);
  JOYX = map(chuck.readJoyX(), -128, 128, 1200, 1800);
  JOYY = map(chuck.readJoyY(), -128, 128, 1200, 1800);
  BTNC = map(chuck.cPressed(), 0, 1, 1200, 1800);
  BTNZ = map(chuck.zPressed(), 0, 1, 1200, 1800); 

  Servo_Position[0] = ROLL;
  Servo_Position[1] = PITCH;
  Servo_Position[2] = JOYX;
  Servo_Position[3] = JOYY;
  Servo_Position[4] = BTNC;
  Servo_Position[5] = BTNZ;
  
 
  
  /*
  Serial.print((int)chuck.readAccelX()); 
  Serial.print(", ");  
  Serial.print((int)chuck.readAccelY()); 
  Serial.print(", ");  
  Serial.print((int)chuck.readAccelZ()); 
  */
  
  
  //uncomment these lines for reading the values by your serial monitor.
  /*  
  Serial.print(chuck.readRoll());
  Serial.print(", ");  
  Serial.print(chuck.readPitch());
  Serial.print(", "); 
  
  Serial.print((int)chuck.readJoyX()); 

  Serial.print(", ");  
  Serial.print((int)chuck.readJoyY()); 

  Serial.print(", ");  
  Serial.print((int)chuck.zPressed()); 
  
  Serial.print(", ");  
  Serial.print((int)chuck.cPressed()); 

  Serial.println();
  */
  


}
